from pybricks.ev3devices import Motor

from main.utils import abs


class MotorController:
    def __init__(self, motor, name="Motor", scale=1):
        self.motor = motor
        self.name = name
        self.scale = scale

    def set_scale(self, scale):
        self.scale = scale

    def set_angle(self, angle):
        """设置马达当前角度
        :param angle: 马达角度，单位为度
        """
        self.motor.reset_angle(angle)
        print("^ [INF] Motor %s angle set to %d." % (self.name, angle))

    def set_speed(self, speed):
        """设置马达速度
        :param speed: 马达速度，单位为度/秒
        """
        self.speed = speed

    def set_unit(self, unit):
        """设置马达旋转单位
        例如：设置为 10 度，则每次旋转以 10 度为单位
        旋转 180 度，则旋转 18 次，每次 10 度
        :param unit: 马达旋转单位，单位为度
        """
        self.unit = unit * self.scale if hasattr(self, "scale") else unit

    def set_safe_range(self, safeRange):
        """设置马达安全范围
        :param safeRange: 马达安全范围，单位为度
        """
        self.safeRange = safeRange
        print(
            "^ [INF] Motor %s safe range set to (%d, %d)."
            % (self.name, safeRange[0], safeRange[1])
        )

    def measure(self, simple=False):
        if simple:
            print(
                "^ [INF] Motor %s is at angle %d and speed %d."
                % (self.name, self.motor.angle(), self.motor.speed())
            )
            return

        print("^ [INF] - Measuring Motor %s ..." % (self.name))
        print("^ [INF]   - Motor angle: ", self.motor.angle())
        print("^ [INF]   - Motor speed: ", self.motor.speed())

    def rotate(self, angle, reverse=False, pauser=lambda: False):
        """
        马达旋转函数
        :param angle: 旋转角度，单位为度
        :param reverse: 是否反向旋转，默认为 False, 即顺时针旋转
        :param pauser: 暂停函数，默认为 lambda: False, 即不暂停
        :return: None
        """

        angle = angle * self.scale if hasattr(self, "scale") else angle

        if self.unit > angle:
            print("^ [ERR] You cann't move once less than 1 degree.")
            return

        steps = int(angle / self.unit)
        steppedAngle = 0
        steppedCount = 0

        print(
            "^ [INF] Moving motor %s degrees with speed %d and unit %d."
            % (angle, self.speed, self.unit)
        )

        while steppedAngle < angle or steppedCount < steps:
            if pauser():
                print(
                    "^ [WAN] Paused by pauser while stepped %d degrees."
                    % (steppedAngle)
                )
                return

            if (angle - steppedAngle) < self.unit:
                self.unit = angle - steppedAngle

            steppedAngle += self.unit
            steppedCount += 1

            self.motor.run_angle(self.speed if not reverse else -self.speed, self.unit)

    def rotate_to(self, target_angle, pauser=lambda: False):
        current_angle = self.motor.angle()
        angle_to_move = target_angle - current_angle
        if angle_to_move == 0:
            return
        self.rotate(abs(angle_to_move), reverse=angle_to_move > 0, pauser=pauser)

    def stop(self):
        self.motor.stop()


class GrabableMotorController(MotorController):
    def __init__(self, motor, name="Motor", scale=1, safeRange=(0, 360)):
        super().__init__(motor, name, scale)
        self.safeRange = safeRange
        self.grab_angle = (self.safeRange[1] - self.safeRange[0]) / 3 * 2

    def grab(self):
        print("^ [INF] Grabbing with motor %s ..." % (self.name))
        self.rotate(self.grab_angle)

    def release(self):
        print("^ [INF] Releasing with motor %s ..." % (self.name))
        self.rotate(self.grab_angle, reverse=True)
